/* 
 * File:   mcQuaternion.h
 *
 * @author Thales Luis Rodrigues Sabino
 * @date   2009
 */

#ifndef _MCQUATERNION_H
#define	_MCQUATERNION_H

#include <math.h>
#include <assert.h>
#include <iostream>
#include <string>

#include "mcVector.h"
#include "mcMatrix.h"

namespace mc {

    template <typename T> class vec2;
    template <typename T> class vec3;
    template <typename T> class vec4;
    
    
    /**
     * @brief Classe template para representação de quaternions
     */
    template <typename T>
    class quaternion {

        /// Atributos
        public:
            union {
                struct {
                    T x;
                    T y;
                    T z;
                    T w;
                };

                T _array[4];
            };

        /// Métodos
        public:

            /**
             * @brief Construtor vazio. Constrói um quaternion nulo
             */
            quaternion( const T &t = T() ) {
                this->x = t;
                this->y = t;
                this->z = t;
                this->w = t;
            }
   
            /**
             * @brief Construtor explícito
             *
             * @param x
             * @param y
             * @param z
             * @param w
             */
            quaternion( const T x, const T y, const T z, const T w ) {
                this->x = x;
                this->y = y;
                this->z = z;
                this->w = w;
            }
    

            /**
             * @brief Construtor por cópia.
             *
             * @param q - Quaternion a ser copiado
             */
            quaternion( const quaternion<T> &q ) {
                this->x = q.x;
                this->y = q.y;
                this->z = q.z;
                this->w = q.w;
            }


            /**
             * @brief Constrói um quaternion a partir de uma matriz 4x4
             *
             * @param m - Matriz
             */
            quaternion( matrix4<T> &m ) {
                this->set_value(m);
            }
        
            
            /**
             * @brief Constrói um quaternion que forma uma rotação em torno de um eixo
             *
             * @param axis - vec3 que representa o eixo
             * @param angle - Ângulo em radianos
             */
            quaternion( const vec3<T> &axis, T angle ) {
                this->set_value(axis, angle);
            }
    
            /**
             * @brief Constrói um quaternion que forma uma rotação de um ponto a outro
             *
             * @param rotateFrom - Origem da rotação
             * @param rotateTo   - Destino da rotação
             */
            quaternion( const vec3<T> &rotateFrom, const vec3<T> &rotateTo ) {
                this->set_value(rotateFrom, rotateTo);
            }
    

            /**
             * @brief Constrói um quaternion por cópia de array
             *
             * @param a - Array
             */
            quaternion( const T *a ) {
                this->x = a[0];
                this->y = a[1];
                this->z = a[2];
                this->w = a[3];
            }

            /**
             * @brief Constrói um quarternion a partir do ângulos de Euler
             *
             * @param picth
             * @param yaw
             * @param roll
             */
            quaternion( const T picth, const T yaw, const T roll ) {
                this->set_value( picth, yaw, roll );
            }


            /**
             * @brief Retorna o tamanho do quaternion
             */
            int get_size() const { return 4; }
            
            /**
             * @brief Retorna a array de dados deste quaternion
             *
             * @return A referência para o array de dados
             */
            const T* get_value() const {
                return &this->_array[0];
            }
       

            /**
             * @brief Retorna o array de dados em elementos separados
             *
             * @param x
             * @param y
             * @param z
             * @param w
             */
            void get_value( T &x, T &y, T &z, T &w ) {
                x = this->x;
                y = this->y;
                z = this->z;
                w = this->w;
            }
   


            /**
             * @brief Seta o valor do quaternion explicitamente
             *
             * @param x
             * @param y
             * @param z
             * @param w
             */
            void set_value( T x, T y, T z, T w ) {
                this->x = x;
                this->y = y;
                this->z = z;
                this->w = w;
            }
  

            /**
             * @brief Seta o valor do quaternion por cópia
             *
             * @param q - Quaternion a ser copiado
             */
            void set_value( const quaternion<T> &q ) {
                this->x = q.x;
                this->y = q.y;
                this->z = q.z;
                this->w = q.w;
            }



            /**
             * @brief Retorna o eixo e o ângulo de rotação deste quaternion
             *
             * @param axis  - Eixo de rotação representado por este quaternion
             * @param angle - Ângulo de rotação em torno do eixo
             */
            void get_value( vec3<T> &axis, T &angle ) const {
                angle = T( acos(this->w) * T(2) );

                if( angle == T(0.0) )
                    axis = vec3<T>( T(0), T(0), T(1) );
                else {
                    axis.x = this->x;
                    axis.y = this->y;
                    axis.z = this->z;

                    normalize<T>(axis);
                }
            }



            /**
             * @brief Obtem uma matriz de rotação 4x4 do quaternion
             *
             * @param m - Matriz de rotação resultante
             */
            void get_value( matrix4<T> &m ) const {
                T norm = x*x + y*y + z*z + w*w;

                T s = ( norm == (T) 0) ? (T) 0 : ( T(2) / norm );

                T xs = x * s;
                T ys = y * s;
                T zs = z * s;

                T wx = w * xs;
                T wy = w * ys;
                T wz = w * zs;

                T xx = x * xs;
                T xy = x * ys;
                T xz = x * zs;

                T yy = y * ys;
                T yz = y * zs;
                T zz = z * zs;

                m._00 = T (1) - ( yy + zz );
                m._10 = xy + wz;
                m._20 = xz - wy;

                m._01 = xy - wz;
                m._11 = T (1) - ( xx + zz );
                m._21 = yz + wx;

                m._02 = xz + wy;
                m._12 = yz - wx;
                m._22 = T (1) - ( xx + yy );

                m._03 = T (0);
                m._13 = T (0);
                m._23 = T (0);
                m._30 = T (0);
                m._31 = T (0);
                m._32 = T (0);
                m._33 = T (1);
            }



            /**
             * @brief Seta o valor do quaternion apartir de um array
             *
             * @param a - Array
             */
            void set_value( const T * a ) const {
                this->x = a[0];
                this->y = a[1];
                this->z = a[2];
                this->w = a[3];
            }
 

            /**
             * @brief Seta o valor do quaternion a partir de uma matriz 4x4
             *
             * @param m - Matriz
             */
            void set_value( matrix4<T> &m ) {
                T tr = m._00 + m._11 + m._22;
                const int nxt[3] = { 1, 2, 0 };

                if( tr > (T) 0 ) {

                    T s = (T) sqrt( tr + m._33 );
                    this->w = ( (T) 0.5 ) / s;

                    this->x = ( m._12 - m._21 ) * s;
                    this->y = ( m._20 - m._02 ) * s;
                    this->z = ( m._01 - m._10 ) * s;

                } else {

                    int i = 0;

                    if( m._11 > m._00 )
                        i = 1;

                    if( m._22 > m.get_element(i,i) )
                        i = 2;

                    int j = nxt[i];
                    int k = nxt[j];

                    T s = (T) sqrt( ( m.get_element(i,j) - ( m.get_element(j,j) + m.get_element(k,k) ) ) + (T) 1 );

                    this->_array[i] = s * (T) 0.5;
                    s = T(0.5 / s);

                    this->w = ( m.get_element(j,k) - m.get_element(k,i) ) * s;
                    this->_array[j] = ( m.get_element(i,j) + m.get_element(j,i) ) * s;
                    this->_array[k] = ( m.get_element(i,k) + m.get_element(k,i) ) * s;
                }
            }



            /**
             * @brief Seta o quaternion para uma rotação em torno de um eixo
             *
             * @param axis  - Eixo
             * @param angle - Ângulo
             */
            void set_value( const vec3<T> &axis, T angle ) {
                T sqr_norm = square_norm( axis );

                if( sqr_norm == (T) 0 ) {

                    // Eixo muito pequeno
                    this->x = (T) 0;
                    this->y = (T) 0;
                    this->z = (T) 0;
                    this->w = (T) 1;

                } else {
                    angle *= (T) 0.5;
                    T sin_theta = (T) sin(angle);

                    if( sqr_norm != (T) 1 )
                        sin_theta /= (T) sqrt( sqr_norm );

                    this->x = sin_theta * axis.x;
                    this->y = sin_theta * axis.y;
                    this->z = sin_theta * axis.z;
                    this->w = (T) cos(angle);
                }                
            }




            /**
             * @brief Seta os valores quaternion que forma uma rotação de um ponto a outro
             *
             * @param rotateFrom - Ponto de origem
             * @param rotateTo   - Ponto de destino
             */
            void set_value( const vec3<T> &rotateFrom, const vec3<T> &rotateTo ) {
                vec3<T> p1;
                vec3<T> p2;

                p1 = normalize( rotateFrom );
                p2 = normalize( rotateTo );

                T alpha = dot( p1, p2 );

                if( alpha == (T) 1 ) {
                    *this = quaternion();
                    return *this;
                }

                if( alpha = (T) -1 ) {
                    vec3<T> v;

                    if( p1.x != p1.y || p1.x != p1.z )
                        v = vec3<T>( p1.y, p1.z, p1.x );
                    else
                        v = vec3<T>( -p1.x, p1.y, p1.z );

                    v -= p1 * dot( p1, v );
                    v = normalize(v);

                    this->set_value( v, (T) M_PI );
                    return *this;
                }
            }



            void set_value(
                const vec3<T> &from_look,
                const vec3<T> &from_up,
                const vec3<T> &to_look,
                const vec3<T> &to_up
            ) 
            {
                quaternion<T> r_look = quaternion(from_look, to_look);

                vec3<T> rotated_from_up( from_up );
                r_look.apply_to_vec3(rotated_from_up);

                quaternion<T> r_twist = quaternion(rotated_from_up, to_up);

                *this = r_twist;
                *this *= r_look;
                return *this;

            }


            /**
             * @brief Constrói um quarternion a partir do ângulos de Euler
             *
             * @param picth
             * @param yaw
             * @param roll
             */
            void set_value( const T pitch, const T yaw, const T roll ) {
                quaternion<T> qx( (T) sin( pitch * 0.5 ), (T) 0, (T) 0, (T) cos( pitch * -0.5) );
                quaternion<T> qy( (T) 0, (T) sin(yaw * -0.5), (T) 0, (T) cos( yaw * -0.5 ) );
                quaternion<T> qz( (T) 0, (T) 0, (T) sin(roll * -0.5), (T) cos(roll * -0.5) );
                qy *= qx;
                this->set_value( qz * qy );
            }




            /**
             * @brief Aplica este quaternion a um vetor 3D
             *
             * @param v - Vetor
             */
            vec3<T>& apply_to_vec3( const vec3<T> &v ) {
                quaternion<T> qi;
                quaternion<T> kr;

                qi = this->conjugate();

                kr.set_value( v.x, v.y, v.z, (T) 0 );
                kr = (*this) * kr;
                kr *= qi;

                vec3<T> *r = new vec3<T>( kr.x, kr.y, kr.z );

                return *r;
            }


            quaternion<T>& conjugate() {
                quaternion<T> *q = new quaternion<T>();
                q->x = -this->x;
                q->y = -this->y;
                q->z = -this->z;
                q->w =  this->w;
                return *q;
            }

            

            /**
             * @brief Escala o quaternion por um fator
             *
             * @param s - Fator de escalamento
             */
            void scale_angle( T s ) {
                vec3<T> axis;
                T angle;

                this->get_value( axis, angle );
                angle *= s;
                this->set_value( axis, angle );
            }


            /**
             * @brief Normaliza o quaternion
             */
            void normalize_quaternion() {
                T rnorm = ((T) 1) / (T) sqrt( x*x + y*y + z*z + w*w );

                this->x *= rnorm;
                this->y *= rnorm;
                this->z *= rnorm;
                this->w *= rnorm;
            }


            /**
             * @brief Normaliza o quaternion
             *
             * @param q - Quaternion a ser normalizado
             */
            void normalize_quaternion( quaternion<T> &q ) {
                this->set_value(q);
                this->normalize_quaternion();
            }
      

            /**
             * @brief Produto de Graham
             *
             * Multiplica dois quaternions através do produto de Graham
             */
            quaternion<T>& operator * ( const quaternion<T> &q ) {
                quaternion<T> *qr = new quaternion();                
                qr->x = q.w*x + q.x*w + q.y*z - q.z*y;
                qr->y = q.w*y + q.y*w + q.z*x - q.x*z;
                qr->z = q.w*z + q.z*w + q.x*y - q.y*x;
                qr->w = q.w*w - q.x*x - q.y*y - q.z*z;
                return *qr;
            }



            /**
             * @brief Produto de Graham
             *
             * Multiplica dois quaternions através do produto de Graham
             */
            quaternion<T>& operator *= ( const quaternion<T> &q ) {
                quaternion<T> *qr = new quaternion();
                qr->w = q.w*w - q.x*x - q.y*y - q.z*z;
                qr->x = q.w*x + q.x*w + q.y*z - q.z*y;
                qr->y = q.w*y + q.y*w + q.z*x - q.x*z;
                qr->z = q.w*z + q.z*w + q.x*y - q.y*x;
                return *qr;
            }

            /**
             * @brief Operador de atribuição
             *
             * @param q
             */
            quaternion<T>& operator = ( const quaternion<T> &q ) {
                this->x = q.x;
                this->y = q.y;
                this->z = q.z;                
                this->w = q.w;
                return *this;
            }
            

            /**
             * @brief Operador de comparação de igualdade
             *
             * @param q1
             * @param q2
             */
            friend bool operator == ( const quaternion<T> &q1, const quaternion<T> q2 ) {
                for( int i = 0; i < q1.get_size(); i++ )
                    if( q1._array[i] != q1._array[i] ) return false;
                
                return true;
            }
            
            /**
             * @brief Operador de comparação de desigualdade
             *
             * @param q1
             * @param q2
             */
            friend bool operator != ( const quaternion<T> &q1, const quaternion<T> q2 ) {
                bool r = true;
                for( int i = 0; i < q1.get_size(); i++ )
                    r &= q1._array[i] == q2._array[i];
                
                return r;
            }


            /**
             * @brief Operador para impressão
             */
            friend std::ostream& operator << ( std::ostream &out, const quaternion<T> &q ) {
                char *pOut = new char[256];
                sprintf( pOut, "(%7.4f, %7.4f, %7.4f, %7.4f)", q.x, q.y, q.z, q.w );
                out << pOut;
                delete [] pOut;
                return out;
            }

    };

};



#endif	/* _MCQUATERNION_H */

